module basic

    implicit none
    real(8), parameter :: pi2 = 6.283185307179586D0

!     interface lstsq
!         module procedure leastsqs, leastsqm
!     end interface ! lstsq

contains

    function mean(x,n)
        real(8), intent(in) :: x(:)
        integer, intent(in) :: n
        real(8) :: mean

        mean = sum(x)/dble(n)
        
    end function mean

    subroutine norm(a,n)

        integer, intent(in) :: n
        real(8), intent(inout) :: a(n)

        integer :: i
        real(8) :: pk, ai

        pk = 0.d0

        do i = 1, n, 1
            ai = abs(a(i))
            if ( ai>pk ) pk = ai
        end do

        do i = 1, n, 1
            a(i) = a(i)/pk
        end do

        return
        
    end subroutine norm

    function dpower(x,n,m)
        
        real(8), intent(in) :: x
        integer, intent(in) :: n, m
        real(8) :: dpower

        integer :: k, l

        if ( m>n ) then
            dpower = 0.d0
            return
        end if

        k = n
        l = m
        dpower = x**(n-m)

        do while ( l>0 )
            dpower = dpower*k
            l = l - 1
            k = k - 1
        end do
        
    end function dpower

    function trapz(y,n,dx,y0)
    ! ================================
    ! 梯形法计算等间距离散数据数值积分
    !
    ! y     --- 离散数据序列
    ! dx    --- 离散数据间距
    ! n     --- 离散点数
    ! y0    --- 积分初值
    ! ================================
        implicit none
        real*8, intent(in) :: dx, y0
        integer, intent(in) :: n
        real*8, intent(in) :: y(:)

        real*8 :: trapz

        integer :: i
        real*8 :: r

        r = 0.d0

        ! Openmp 并行指令
        !$OMP PARALLEL PRIVATE(i), SHARED(y,dx,n)
        !$OMP DO REDUCTION(+:r)

        do i = 2, n, 1
            r = r + dx*(y(i)+y(i-1))
        end do

        !$OMP END PARALLEL

        trapz = y0+r*0.5
        return

    end function trapz

    subroutine cumtrapz(y,z,n,dx,z0)
    ! ================================
    ! 梯形法计算等间距离散数据累积积分
    ! ================================

        real*8, intent(in) :: dx, z0
        integer, intent(in) :: n
        real*8, intent(in) :: y(:)
        real*8, intent(out) :: z(:)

        integer :: i
        real*8 :: r

        z(1) = z0
        r = 0.D0
        do i = 2, n, 1
            r = r + dx*(y(i)+y(i-1))*0.5
            z(i) = z0 + r
        end do
        return

    end subroutine cumtrapz

    subroutine fftfreqs(Nfft,fs,freqs)

        integer, intent(in) :: Nfft
        real(8), intent(in) :: fs
        real(8), intent(out) :: freqs(:)

        real(8) :: fn, df
        integer :: i, l

        fn = 0.5D0*fs
        df = fs/dble(Nfft)
        freqs = 0.0D0

        if ( mod(Nfft, 2) == 0 ) then
            l = Nfft/2-1
            freqs(2:Nfft/2) = [ ( (dble(i)*df), i=1,l ) ]
            freqs(Nfft/2+1:Nfft) = [ ( -fn+(dble(i-1)*df), i=1,l+1 ) ]
        else
            l = (Nfft+1)/2-1
            freqs(2:(Nfft+1)/2) = [ ( (dble(i)*df), i=1,l ) ]
            freqs((Nfft+1)/2+1:Nfft) = [ ( -(dble(l-i+1)*df), i=1,l ) ]
        end if

        return

    end subroutine fftfreqs

    subroutine acc2vd(a,v,d,n,dt,v0,d0)

        real(8), intent(in) :: dt,v0,d0
        integer(4), intent(in) :: n
        real(8), intent(in) :: a(n)
        real(8), intent(out) :: v(n), d(n)

        integer(4) :: i
        real(8) :: vi, di

        v(1) = v0
        d(1) = d0

        vi = 0.D0
        di = 0.D0
        do i = 2, n, 1
            vi = vi + dt*(a(i)+a(i-1))*0.5
            v(i) = v0 + vi
            di = di + dt*(v(i)+v(i-1))*0.5
            d(i) = d0 + di
        end do
        return

    end subroutine acc2vd

    subroutine ratacc2vd(a,v,d,n,dt,v0,d0)

        real(8), intent(in) :: dt,v0,d0
        integer(4), intent(in) :: n
        real(8), intent(in) :: a(n)
        real(8), intent(out) :: v(n), d(n)

        real(8) :: vn, dn
        integer :: i

        call acc2vd(a,v,d,n,dt,v0,d0)
        vn = sum(v)/dble(n)
        dn = 0.0d0
        do i = 1, n, 1
            dn = dn + d(i) - vn*dble(i-1)*dt
        end do
        dn = dn/dble(n)

        call acc2vd(a,v,d,n,dt,v0-vn,d0-dn)
        
    end subroutine ratacc2vd

    subroutine leastsqs(a,b,m,n)

        implicit none
        real*8, intent(in) :: a(:,:)
        real*8, intent(inout) :: b(:)
        integer, intent(in) :: m, n

        integer :: i, nrhs, lda, ldb, lwork
        integer :: rank, info
        real*8 :: twork(1)
        integer :: tiwork(1)
        real*8, allocatable :: s(:)
        real*8, allocatable :: work(:)
        integer,allocatable :: iwork(:)

        ! m = size(a, dim=1)
        ! n = size(a, dim=2)
        nrhs = 1

        lda = max(1,m)
        ldb = max(1,m,n)

        allocate(s(min(m,n)))

        lwork = -1

        call dgelsd(m, n, nrhs, a, lda, b, ldb, s, -1.d0, &
            rank, twork, lwork, tiwork, info)

        lwork = int(twork(1))
        allocate(work(lwork))
        allocate(iwork(tiwork(1)))

        call dgelsd(m, n, nrhs, a, lda, b, ldb, s, -1.d0, &
            rank, work, lwork, iwork, info)

        deallocate(s)
        deallocate(work)
        deallocate(iwork)

        return

    end subroutine leastsqs

    subroutine leastsqm(a,b,m,n,nrhs)

        implicit none
        real*8, intent(in) :: a(:,:)
        real*8, intent(inout) :: b(:,:)
        integer, intent(in) :: m, n, nrhs

        integer :: i, lda, ldb, lwork
        integer :: rank, info
        real*8 :: twork(1)
        integer :: tiwork(1)
        real*8, allocatable :: s(:)
        real*8, allocatable :: work(:)
        integer,allocatable :: iwork(:)

        ! m = size(a, dim=1)
        ! n = size(a, dim=2)
        ! nrhs = size(b, dim=2)

        lda = max(1,m)
        ldb = max(1,m,n)

        allocate(s(min(m,n)))

        lwork = -1

        call dgelsd(m, n, nrhs, a, lda, b, ldb, s, -1.d0, &
            rank, twork, lwork, tiwork, info)

        lwork = int(twork(1))
        allocate(work(lwork))
        allocate(iwork(tiwork(1)))

        call dgelsd(m, n, nrhs, a, lda, b, ldb, s, -1.d0, &
            rank, work, lwork, iwork, info)

        deallocate(s)
        deallocate(work)
        deallocate(iwork)

        return

    end subroutine leastsqm

    subroutine error(y,y0,n,aerror,merror)
        integer, intent(in) :: n
        real(8), intent(in) :: y(:), y0(:)
        real(8), intent(out) :: aerror, merror

        real(8), allocatable :: e(:)

        allocate(e(n-2))

        e = (y(2:n-1) - y0(2:n-1))/y0(2:n-1)

        aerror = sqrt(sum(e*e)/dble(n-2))
        merror = maxval(abs(e), dim=1)

        deallocate(e)
        
    end subroutine error

    subroutine errora(y,y0,n,aerror,merror)
        integer, intent(in) :: n
        real(8), intent(in) :: y(:), y0(:)
        real(8), intent(out) :: aerror, merror

        real(8), allocatable :: e(:)

        allocate(e(n))

        e = (y - y0)/y0

        aerror = sqrt(sum(e*e)/dble(n))
        merror = maxval(abs(e), dim=1)

        deallocate(e)
        
    end subroutine errora

    subroutine incrlininterp(x,y,n,xi,yi,ni)
        integer, intent(in) :: n, ni
        real(8), intent(in) :: x(:), y(:), xi(:)
        real(8), intent(out) :: yi(:)

        integer :: i, j
        real(8) :: xc, yc, xp, yp, slope

        xp = x(1)
        yp = y(1)
        j = 1
        do i = 2, n, 1
            xc = x(i)
            yc = y(i)
            slope = (yc - yp)/(xc - xp)
            do while ( xi(j)<=xc .and. j<=ni)
                yi(j) = yp + slope*(xi(j)-xp)
                j = j + 1
            end do
            xp = xc
            yp = yc
        end do
        
    end subroutine incrlininterp

    subroutine incrfindfirst(a,n,x,i)
        integer, intent(in) :: n
        real(8), intent(in) :: a(:), x
        integer, intent(out) :: i

        i = 1
        do while ( a(i)<x .and. i<=n )
            i = i + 1
        end do

        return
        
    end subroutine incrfindfirst

    subroutine incrfindlast(a,n,x,i)
        integer, intent(in) :: n
        real(8), intent(in) :: a(:), x
        integer, intent(out) :: i

        i = n
        do while ( a(i)>x .and. i>=1 )
            i = i - 1
        end do

        return
        
    end subroutine incrfindlast

    subroutine decrlininterp(x,y,n,xi,yi,ni)
        integer, intent(in) :: n, ni
        real(8), intent(in) :: x(:), y(:), xi(:)
        real(8), intent(out) :: yi(:)

        integer :: i, j
        real(8) :: xc, yc, xp, yp, slope

        xp = x(n)
        yp = y(n)
        j = 1
        do i = 1, n-1, 1
            xc = x(n-i)
            yc = y(n-i)
            slope = (yc - yp)/(xc - xp)
            do while ( xi(j)>=xc .and. j<=ni)
                yi(j) = yp + slope*(xi(j)-xp)
                j = j + 1
            end do
            xp = xc
            yp = yc
        end do
        
    end subroutine decrlininterp

    subroutine decrfindfirst(a,n,x,i)
        integer, intent(in) :: n
        real(8), intent(in) :: a(:), x
        integer, intent(out) :: i

        i = 1
        do while ( a(i)>x .and. i<=n )
            i = i + 1
        end do

        return
        
    end subroutine decrfindfirst

    subroutine decrfindlast(a,n,x,i)
        integer, intent(in) :: n
        real(8), intent(in) :: a(:), x
        integer, intent(out) :: i

        i = n
        do while ( a(i)<x .and. i>=1 )
            i = i - 1
        end do

        return

    end subroutine decrfindlast


end module basic

module polynomial

    use basic
    implicit none

    interface polyval
        module procedure polyvals, polyvalv
    end interface ! polyval

contains

    subroutine polyvals(p,m,x,y)

        real*8, intent(in) :: p(:), x
        integer, intent(in) :: m
        real*8, intent(out) :: y

        integer :: i, n

        n = idnz(p)

        y = 0.D0
        do i = n, m, 1
            y = x*y + p(i)
        end do

        return

    end subroutine polyvals

    subroutine polyvalv(p,m,x,y,n)

        real*8, intent(in) :: p(:), x(:)
        integer, intent(in) :: m, n
        real*8, intent(out) :: y(:)
        integer :: i, j, k

        k = idnz(p)

        !$OMP PARALLEL DEFAULT(NONE), PRIVATE(i,j), SHARED(p,m,x,y,n,k)
        !$OMP DO
        do i = 1, n, 1
            y(i) = 0.d0
            do j = k, m, 1
                y(i) = x(i)*y(i) + p(j)
            end do
        end do
        !$OMP END PARALLEL

        return
    end subroutine polyvalv

    subroutine polyder(p,m,n,r,l)

        real*8, intent(in) :: p(:)
        integer, intent(in) :: m,n
        real*8, intent(inout) :: r(:)
        integer, intent(inout) :: l

        integer :: i, j, k

        if ( l<0 ) then
            l = m - n
            if ( l<=0 ) l = 1
            return
        end if

        if ( n>=m ) then
            r(1) = 0.d0
            return
        end if

        do i = 1, l, 1
            r(i) = p(i)
            k = m-i
            do j = 1, n, 1
                r(i) = r(i)*k
                k = k-1
            end do
        end do
        return

    end subroutine polyder

    subroutine polyint(p,m,n,r,l)

        real*8, intent(in) :: p(:)
        integer, intent(in) :: m,n
        real*8, intent(inout) :: r(:)
        integer, intent(inout) :: l

        integer :: i, j, k

        if ( l<0 ) then
            l = m + n
            return
        end if

        r(m+1:l) = 0.d0

        do i = 1, m, 1
            r(i) = p(i)
            k = m-i
            do j = 1, n, 1
                r(i) = r(i)/dble(k+1)
                k = k+1
            end do
        end do
        return
    end subroutine polyint

    subroutine polymul(p,m,q,n,r,l)

        real*8, intent(in) :: p(:),q(:)
        integer, intent(in) :: m,n
        real*8, intent(inout) :: r(:)
        integer, intent(inout) :: l

        integer :: i, j

        if ( l<0 ) then
            l = m + n - 1
            return
        end if

        do i = 1, l, 1
            r(i) = 0.d0
            do j = 1, m, 1
                if (i-j > -1 .and. i-j < n) &
                    r(i) = r(i) + p(i-j+1)*q(j)
            end do
        end do

        return

    end subroutine polymul

    subroutine polydiv(p,m,q,n,r,l)

        real*8, intent(in) :: p(:),q(:)
        integer, intent(in) :: m,n
        real*8, intent(inout) :: r(:)
        integer, intent(inout) :: l

        integer :: i, j

        if ( l<0 ) then
            l = m + n - 1
            return
        end if

        do i = 1, l, 1
            r(i) = 0.d0
            do j = 1, m, 1
                if (i-j > -1 .and. i-j < n) &
                    r(i) = r(i) + p(i-j+1)*q(j)
            end do
        end do

        return

    end subroutine polydiv

    subroutine polyadd(p,m,q,n,r,l)

        real*8, intent(in) :: p(:),q(:)
        integer, intent(in) :: m,n
        real*8, intent(inout) :: r(:)
        integer, intent(inout) :: l

        integer :: i, j

        if ( l<0 ) then
            l = max(m,n)
            return
        end if

        if ( m>=n ) then
            do i = 1, l, 1
                if ( i<=m-n ) then
                    r(i) = p(i)
                else
                    r(i) = p(i) + q(i-m+n)
                end if
            end do
        else
            do i = 1, l, 1
                if ( i<=n-m ) then
                    r(i) = q(i)
                else
                    r(i) = q(i) + p(i-n+m)
                end if
            end do
        end if

        return

    end subroutine polyadd

    subroutine polyroots(p,m,r,l)
        !> Find roots of the (m-1)-order polynomial by solving the eigen
        ! problem of the coresponding companion matrix.
        !> The roots are all in complex form.

        real*8, intent(in) :: p(:)
        integer, intent(in) :: m
        complex*16, intent(inout) :: r(:)
        integer, intent(inout) :: l

        real*8, allocatable :: A(:,:), work(:), wr(:), wi(:)
        real*8 :: temp(1)
        integer :: i, j, info, lwork

        if ( l<0 ) then
            l = m - 1
            if ( l<1 ) stop "Error: no roots; order too low (<1)!"
            return
        end if

        if ( m == 2 ) then
            r(1) = -p(2)/p(1)
            return
        end if

        allocate(A(l,l))
        allocate(wr(l))
        allocate(wi(l))

        A = 0.d0
        A(1,1) = -p(2)/p(1)
        do i = 2, l, 1
            A(i,i-1) = 1.d0
            A(1,i) = -p(i+1)/p(1)
        end do

        lwork = -1
        call dgeev ( "N", "N", l, A, l, wr, wi, temp, l, temp, l, temp, lwork, info )
        lwork = int(temp(1))
        allocate(work(lwork))
        call dgeev ( "N", "N", l, A, l, wr, wi, temp, l, temp, l, work, lwork, info )

        do i = 1, l, 1
            r(i) = cmplx(wr(i),wi(i))
        end do

        deallocate(A)
        deallocate(work)
        deallocate(wr)
        deallocate(wi)

        return

    end subroutine polyroots


    function idnz(p)
        !> Return the index of first non-zero element in the array
        real*8, intent(in) :: p(:)
        integer :: idnz

        idnz = 1

        do while ( p(idnz)==0.d0 )
            idnz = idnz + 1
        end do
        return
    end function idnz

    subroutine polyfit(a,t,n,c,oh,ol)

        integer, intent(in) :: n
        real(8), intent(in) :: a(:), t(:)
        integer, intent(in) :: oh,ol
        real(8), intent(out) :: c(:)

        integer :: i, j, k
        real(8), allocatable :: b(:)
        real(8), allocatable :: M(:,:)

        k = oh-ol+1
        allocate(M(n,k))
        allocate(b(n))

        c = 0.D0
        b = a

        !$OMP PARALLEL DEFAULT(NONE), PRIVATE(i,j), SHARED(M,t,n,k,oh)
        !$OMP DO
        do i = 1, n, 1
            do j = 1, k, 1
                M(i,j) = t(i)**(oh-j+1)
            end do
        end do
        !$OMP END PARALLEL

        call leastsqs(M,b,n,k)

        c(1:k) = b(1:k)

        deallocate(M)
        deallocate(b)

        return

    end subroutine polyfit

    subroutine polydetrend(a,n,oh,ol)

        integer(4), intent(in) :: n
        real(8), intent(inout) :: a(n)
        integer(4), intent(in) :: oh,ol

        integer :: i
        real(8), allocatable :: t(:), c(:), ac(:)

        if ( oh == 0 ) then
            a = a - sum(a)/dble(n)
            return
        end if

        allocate(t(n))
        allocate(ac(n))
        allocate(c(oh+1))

        t = [ (((i-1)*1.D0),i=1,n) ]
        call polyfit(a,t,n,c,oh,ol)
        call polyval(c,oh+1,t,ac,n)

        a = a - ac

        deallocate(t)
        deallocate(c)
        deallocate(ac)

        return

    end subroutine polydetrend

    subroutine polyblc(a,n,oh,ol,dt,v0,d0)
        integer, intent(in) :: n
        real(8), intent(in) :: dt,v0,d0
        real(8), intent(inout) :: a(n)
        integer, intent(in) :: oh,ol

        integer :: i,l
        real(8), allocatable :: t(:), v(:), d(:), c(:), r(:), ac(:)

        allocate(t(n))
        allocate(v(n))
        allocate(d(n))
        allocate(ac(n))

        allocate(c(oh+2))
        l = oh+1
        allocate(r(l))

        t = [ (((i-1)*dt),i=1,n) ]

        call cumtrapz(a,v,n,dt,v0)
        call polyfit(v,t,n,c,oh+1,ol+1)
        call polyder(c,oh+2,1,r,l)
        call polyval(r,oh+1,t,ac,n)
        a = a - ac

        deallocate(c)
        allocate(c(oh+3))

        call acc2vd(a,v,d,n,dt,v0,d0)
        call polyfit(d,t,n,c,oh+2,ol+2)
        call polyder(c,oh+3,2,r,l)
        call polyval(r,oh+1,t,ac,n)

        a = a - ac

        deallocate(t)
        deallocate(ac)
        deallocate(v)
        deallocate(d)
        deallocate(c)
        deallocate(r)

        return
    end subroutine polyblc

end module polynomial

module spectra

    use, intrinsic :: iso_c_binding
    use basic
    implicit none
    include "fftw3.f03"
    integer, parameter :: MPR = 50

contains

    function nextpow2(n)
        integer, intent(in) :: n
        integer :: nextpow2

        nextpow2 = 1

        do while ( nextpow2<n )
            nextpow2 = nextpow2*2
        end do
        return

    end function nextpow2

    subroutine spamixed(acc,n,dt,zeta,P,nP,SPA,INDSPA)

        integer, intent(in) :: n, nP
        real(C_DOUBLE), intent(in) :: dt, zeta
        real(C_DOUBLE), intent(in) :: acc(:), P(:)
        real(C_DOUBLE), intent(out) :: SPA(:)
        integer, intent(out) :: INDSPA(:)

        integer :: m

        m = 1
        do while ( P(m)<=MPR*dt )
            m = m + 1
        end do

        call spafreq(acc,n,dt,zeta,P(1:m),m,SPA(1:m),INDSPA(1:m))
        if ( m<nP ) then
            call spanmk(acc,n,dt,zeta,P(m+1:nP),(nP-m),SPA(m+1:nP),INDSPA(m+1:nP))
        end if

    end subroutine spamixed

    subroutine pspamixed(acc,n,dt,zeta,P,nP,SPA,INDSPA)

        integer, intent(in) :: n, nP
        real(C_DOUBLE), intent(in) :: dt, zeta
        real(C_DOUBLE), intent(in) :: acc(:), P(:)
        real(C_DOUBLE), intent(out) :: SPA(:)
        integer, intent(out) :: INDSPA(:)

        integer :: m

        m = 1

        do while ( P(m)<=MPR*dt )
            m = m + 1
        end do

        call pspafreq(acc,n,dt,zeta,P(1:m),m,SPA(1:m),INDSPA(1:m))
        if ( m<nP ) then
            call pspanmk(acc,n,dt,zeta,P(m+1:nP),(nP-m),SPA(m+1:nP),INDSPA(m+1:nP))
        end if

    end subroutine pspamixed

    subroutine spavdmixed(acc,n,dt,zeta,P,nP,SPA,INDSPA,SPV,SPD,SPEV)

        integer, intent(in) :: n, nP
        real(C_DOUBLE), intent(in) :: dt, zeta
        real(C_DOUBLE), intent(in) :: acc(:), P(:)
        real(C_DOUBLE), intent(out) :: SPA(:), SPV(:), SPD(:), SPEV(:)
        integer, intent(out) :: INDSPA(:)

        integer :: m
        m = 1
        do while ( P(m)<=MPR*dt )
            m = m + 1
        end do

        call spavdfreq(acc,n,dt,zeta,P(1:m),m,SPA(1:m),INDSPA(1:m),&
            SPV(1:m),SPD(1:m),SPEV(1:m))
        if ( m<nP ) then
            call spavdnmk(acc,n,dt,zeta,P(m+1:nP),(nP-m),SPA(m+1:nP),&
                INDSPA(m+1:nP),SPV(m+1:nP),SPD(m+1:nP),SPEV(m+1:nP))
        end if

    end subroutine spavdmixed

    subroutine spafreq(acc,n,dt,zeta,P,nP,SPA,INDSPA)

        integer, intent(in) :: n, nP
        real(C_DOUBLE), intent(in) :: dt, zeta
        real(C_DOUBLE), intent(in) :: acc(:), P(:)
        real(C_DOUBLE), intent(out) :: SPA(:)
        integer, intent(out) :: INDSPA(:)

        complex(C_DOUBLE_COMPLEX), allocatable :: af(:), raf(:)
        real(C_DOUBLE), allocatable :: a0(:), ra(:), w(:), w0(:)
        integer :: i, j, Nfft
        complex(C_DOUBLE_COMPLEX) :: IONE=cmplx(0.d0,1.d0)
        real(C_DOUBLE) :: w0i2, w0iwj, wj2
        type(C_PTR) :: plan, iplan

        SPA = 1.D0
        INDSPA = 1

        Nfft = nextpow2(n)

        allocate(a0(Nfft))
        allocate(af(Nfft))
        allocate(raf(Nfft))
        allocate(ra(Nfft))
        allocate(w(Nfft))
        allocate(w0(nP))

        call fftfreqs(Nfft,1.d0/dt,w)
        w = w*pi2
        w0 = pi2/P

        ra = 0.d0
        a0 = 0.d0
        a0(1:n) = acc

        plan  = fftw_plan_dft_r2c_1d(Nfft, a0, af, FFTW_ESTIMATE)
        iplan = fftw_plan_dft_c2r_1d(Nfft, af, ra, FFTW_ESTIMATE)
        call fftw_execute_dft_r2c(plan, a0, af)

        do i = 1, nP, 1
            w0i2 = w0(i)*w0(i)
            do j = 1, Nfft/2+1, 1
                w0iwj = w0(i)*w(j)
                wj2 = w(j)*w(j)
                raf(j) = af(j)*(w0i2+2.d0*zeta*w0iwj*IONE)/&
                        (w0i2-wj2+2.d0*zeta*w0iwj*IONE)
            end do
            call fftw_execute_dft_c2r(iplan, raf, ra)

            INDSPA(i) = maxloc(dabs(ra(1:n)),1)
            SPA(i) = ra(INDSPA(i))/dble(Nfft)
        end do

        call fftw_destroy_plan(plan)
        call fftw_destroy_plan(iplan)

        deallocate( a0)
        deallocate( af)
        deallocate(raf)
        deallocate( ra)
        deallocate(  w)
        deallocate( w0)

        return

    end subroutine spafreq

    subroutine pspafreq(acc,n,dt,zeta,P,nP,SPA,INDSPA)

        integer, intent(in) :: n, nP
        real(C_DOUBLE), intent(in) :: dt, zeta
        real(C_DOUBLE), intent(in) :: acc(:), P(:)
        real(C_DOUBLE), intent(out) :: SPA(:)
        integer, intent(out) :: INDSPA(:)

        complex(C_DOUBLE_COMPLEX), allocatable :: af(:), raf(:)
        real(C_DOUBLE), allocatable :: a0(:), ra(:), w(:), w0(:)
        integer :: i, j, Nfft
        complex(C_DOUBLE_COMPLEX) :: IONE=cmplx(0.d0,1.d0)
        real(C_DOUBLE) :: w0i2, w0iwj, wj2
        type(C_PTR) :: plan, iplan

        SPA = 1.D0
        INDSPA = 1

        Nfft = nextpow2(n)

        allocate(a0(Nfft))
        allocate(af(Nfft))
        allocate(raf(Nfft))
        allocate(ra(Nfft))
        allocate(w(Nfft))
        allocate(w0(nP))

        call fftfreqs(Nfft,1.d0/dt,w)
        w = w*pi2
        w0 = pi2/P

        ra = 0.d0
        a0 = 0.d0
        a0(1:n) = acc

        plan  = fftw_plan_dft_r2c_1d(Nfft, a0, af, FFTW_ESTIMATE)
        iplan = fftw_plan_dft_c2r_1d(Nfft, af, ra, FFTW_ESTIMATE)
        call fftw_execute_dft_r2c(plan, a0, af)

        do i = 1, nP, 1
            w0i2 = w0(i)*w0(i)
            do j = 1, Nfft/2+1, 1
                w0iwj = w0(i)*w(j)
                wj2 = w(j)*w(j)
                raf(j) = af(j)*(-1.d0/(w0i2-wj2+2.d0*zeta*w0iwj*IONE))
            end do
            call fftw_execute_dft_c2r(iplan, raf, ra)

            INDSPA(i) = maxloc(dabs(ra(1:n)),1)
            SPA(i) = ra(INDSPA(i))/dble(Nfft)*39.47841760435743D0/P(i)/P(i)
        end do

        call fftw_destroy_plan(plan)
        call fftw_destroy_plan(iplan)

        deallocate( a0)
        deallocate( af)
        deallocate(raf)
        deallocate( ra)
        deallocate(  w)
        deallocate( w0)

        return

    end subroutine pspafreq

    subroutine spavdfreq(acc,n,dt,zeta,P,nP,SPA,INDSPA,SPV,SPD,SPEV)

        integer, intent(in) :: n, nP
        real(C_DOUBLE), intent(in) :: dt, zeta
        real(C_DOUBLE), intent(in) :: acc(:), P(:)
        real(C_DOUBLE), intent(out) :: SPA(:), SPV(:), SPD(:), SPEV(:)
        integer, intent(out) :: INDSPA(:)

        complex(C_DOUBLE_COMPLEX), allocatable :: af(:), raf(:), rvf(:), rdf(:)
        real(C_DOUBLE), allocatable :: a0(:), ra(:), w(:), w0(:)
        real(C_DOUBLE), allocatable :: rv(:), rd(:)
        integer :: i, j, Nfft
        complex(C_DOUBLE_COMPLEX) :: IONE=cmplx(0.d0,1.d0)
        real(C_DOUBLE) :: w0i2, w0iwj, wj2, iNfft
        type(C_PTR) :: plan, iplan

        SPA = 1.D0
        SPV = 1.D0
        SPD = 1.D0
        INDSPA = 1

        Nfft = nextpow2(n)
        iNfft = 1.d0/dble(Nfft)

        allocate(a0(Nfft))
        allocate(af(Nfft))
        allocate(raf(Nfft))
        allocate(rvf(Nfft))
        allocate(rdf(Nfft))
        allocate(ra(Nfft))
        allocate(rv(Nfft))
        allocate(rd(Nfft))
        allocate(w(Nfft))
        allocate(w0(nP))

        call fftfreqs(Nfft,1.d0/dt,w)
        w = w*pi2
        w0 = pi2/P

        ra = 0.d0
        rv = 0.d0
        rd = 0.d0
        a0 = 0.d0
        a0(1:n) = acc

        plan  = fftw_plan_dft_r2c_1d(Nfft, a0, af, FFTW_ESTIMATE)
        iplan = fftw_plan_dft_c2r_1d(Nfft, af, ra, FFTW_ESTIMATE)
        call fftw_execute_dft_r2c(plan, a0, af)

        do i = 1, nP, 1
            w0i2 = w0(i)*w0(i)
            do j = 1, Nfft/2+1, 1
                w0iwj = w0(i)*w(j)
                wj2 = w(j)*w(j)
                raf(j) = af(j)*(w0i2+2.D0*zeta*w0iwj*IONE)/&
                        (w0i2-wj2+2.d0*zeta*w0iwj*IONE)
                rvf(j) = af(j)*(-w(j)*IONE/(w0i2-wj2+2.d0*zeta*w0iwj*IONE))
                rdf(j) = af(j)*(-1.d0/(w0i2-wj2+2.d0*zeta*w0iwj*IONE))
            end do
            call fftw_execute_dft_c2r(iplan, raf, ra)
            call fftw_execute_dft_c2r(iplan, rvf, rv)
            call fftw_execute_dft_c2r(iplan, rdf, rd)
            INDSPA(i) = maxloc(dabs(ra(1:n)),1)
            SPA(i) = dabs(ra(INDSPA(i)))*iNfft
            SPV(i) = maxval(dabs(rv(1:n)))*iNfft
            SPD(i) = maxval(dabs(rd(1:n)))*iNfft
            SPEV(i) = sqrt(2.d0*trapz(-rv(1:n)*acc*iNfft,n,dt,0.d0))
        end do

        call fftw_destroy_plan(plan)
        call fftw_destroy_plan(iplan)

        deallocate( a0)
        deallocate( af)
        deallocate(raf)
        deallocate(rvf)
        deallocate(rdf)
        deallocate( ra)
        deallocate( rv)
        deallocate( rd)
        deallocate(  w)
        deallocate( w0)

        return

    end subroutine spavdfreq

    subroutine spanmk(acc,n,dt,zeta,P,nP,SPA,INDSPA)

        integer, intent(in) :: n, nP
        real(C_DOUBLE), intent(in) :: dt, zeta
        real(C_DOUBLE), intent(in) :: acc(:), P(:)
        real(C_DOUBLE), intent(out) :: SPA(:)
        integer, intent(out) :: INDSPA(:)

        integer :: k
        real(8) :: SPVk, SPDk, SPEVk

        do k = 1, nP, 1
            call newmark(acc,n,dt,zeta,P(k),SPA(k),INDSPA(k),SPVk,SPDk,SPEVk)
        end do

    end subroutine spanmk

    subroutine pspanmk(acc,n,dt,zeta,P,nP,SPA,INDSPA)

        integer, intent(in) :: n, nP
        real(C_DOUBLE), intent(in) :: dt, zeta
        real(C_DOUBLE), intent(in) :: acc(:), P(:)
        real(C_DOUBLE), intent(out) :: SPA(:)
        integer, intent(out) :: INDSPA(:)

        integer :: k
        real(8) :: SPVk, SPDk, SPEVk

        do k = 1, nP, 1
            call newmark(acc,n,dt,zeta,P(k),SPDk,INDSPA(k),SPVk,SPA(k),SPEVk)
            SPA(k) = SPA(k)*39.47841760435743D0/P(k)/P(k)
        end do

    end subroutine pspanmk

    subroutine spavdnmk(acc,n,dt,zeta,P,nP,SPA,INDSPA,SPV,SPD,SPEV)

        integer, intent(in) :: n, nP
        real(C_DOUBLE), intent(in) :: dt, zeta
        real(C_DOUBLE), intent(in) :: acc(:), P(:)
        real(C_DOUBLE), intent(out) :: SPA(:), SPV(:), SPD(:), SPEV(:)
        integer, intent(out) :: INDSPA(:)

        integer :: k

        do k = 1, nP, 1
            call newmark(acc,n,dt,zeta,P(k),SPA(k),INDSPA(k),SPV(k),SPD(k),SPEV(k))
        end do

    end subroutine spavdnmk

    subroutine newmark(acc,n,dt0,zeta,P,SPA,INDSPA,SPV,SPD,SPEV)

        integer, intent(in) :: n
        real(8), intent(in) :: dt0, zeta
        real(8), intent(in) :: acc(:), P
        real(8), intent(out) :: SPA, SPV, SPD, SPEV
        integer, intent(out) :: INDSPA

        real(8) :: rc(3),rl(3),ac,al,aa,ein,da
        real(8) :: beta = 1.d0/4.d0, gamma = 0.5d0
        real(8) :: k,c,w,keff,kinv,feff,dt,rvl
        real(8) :: b1, b2, b3, b4, b5, b6, b7, b8
        integer :: i,j,r

        if ( dt0*MPR > P ) then
            r  = ceiling(MPR*dt0/P)
            dt = dt0/r
        else
            r = 1
            dt = dt0
        end if

        b1 = 1.d0/(beta*dt*dt)
        b2 = 1.d0/(beta*dt)
        b3 = 1.d0/(beta*2.0d0)-1.d0
        b4 = gamma/(beta*dt)
        b5 = gamma/beta-1.d0
        b6 = 0.5d0*dt*(gamma/beta-2.0d0)
        b7 = dt*(1.0d0-gamma)
        b8 = dt*gamma

        w = pi2/P
        k = w**2.d0
        c = 2.0d0*zeta*w
        keff = k+b1+b4*c
        kinv = 1.0d0/keff

        !  initiation
        rl = 0.d0
        rc = 0.d0
        SPA = 0.0d0
        SPV = 0.0d0
        SPD = 0.0d0
        INDSPA = 0
        ein = 0.0d0
        al = 0.d0
        rvl = 0.d0

        !  computation of response
        do i = 1,n,1

            ac = acc(i)
            da = (ac - al)/dble(r)

            do j = 1, r, 1
                ac = al + da*j
                feff = ac+(b1*rl(1)+b2*rl(2)+b3*rl(3))&
                +c*(b4*rl(1)+b5*rl(2)+b6*rl(3))
                rc(1) = feff*kinv
                rc(2) = b4*(rc(1)-rl(1))-b5*rl(2)-b6*rl(3)
                rc(3) = ac-k*rc(1)-c*rc(2)
                rl(1) = rc(1)
                rl(2) = rc(2)
                rl(3) = rc(3)
            end do

            ein = ein+0.5d0*dt0*(rvl*al+rc(2)*ac)
            aa = -rc(3)+ac

            if (abs(aa) > abs(SPA)) then
                SPA=aa
                INDSPA=i
            endif

            if (abs(rc(2)) > abs(SPV)) then
                SPV=rc(2)
            endif

            if (abs(rc(1)) > abs(SPD)) then
                SPD=rc(1)
            endif

            al = ac
            rvl = rc(2)

        end do
        SPEV = sqrt(2.d0*ein)
        return

    end subroutine newmark

    subroutine rnmk(acc,n,dt0,zeta,P,ra,rv,rd)

        integer, intent(in) :: n
        real(8), intent(in) :: dt0, zeta
        real(8), intent(in) :: acc(:), P
        real(8), intent(out) :: ra(:),rv(:),rd(:)

        real(8) :: rc(3),rl(3),ac,al,aa,da
        real(8) :: beta = 1.d0/4.d0, gamma = 0.5d0
        real(8) :: k,c,w,keff,kinv,feff,dt,rvl
        real(8) :: b1, b2, b3, b4, b5, b6, b7, b8
        integer :: i,j,r

        if ( dt0*MPR > P ) then
            r  = ceiling(MPR*dt0/P)
            dt = dt0/r
        else
            r = 1
            dt = dt0
        end if

        b1 = 1.d0/(beta*dt*dt)
        b2 = 1.d0/(beta*dt)
        b3 = 1.d0/(beta*2.0d0)-1.d0
        b4 = gamma/(beta*dt)
        b5 = gamma/beta-1.d0
        b6 = 0.5d0*dt*(gamma/beta-2.0d0)
        b7 = dt*(1.0d0-gamma)
        b8 = dt*gamma

        w = 6.283185307179586D0/P
        k = w**2.d0
        c = 2.0d0*zeta*w
        keff = k+b1+b4*c
        kinv = 1.0d0/keff

        !  initiation
        rl = 0.d0
        rc = 0.d0
        ra = 0.d0
        rv = 0.d0
        rd = 0.d0
        al = 0.d0

        !  computation of response
        do i = 1, n, 1

            ac = acc(i)
            da = (ac - al)/dble(r)

            do j = 1, r, 1
                ac = al + da*j
                feff = ac+(b1*rl(1)+b2*rl(2)+b3*rl(3))&
                +c*(b4*rl(1)+b5*rl(2)+b6*rl(3))
                rc(1) = feff*kinv
                rc(2) = b4*(rc(1)-rl(1))-b5*rl(2)-b6*rl(3)
                rc(3) = ac-k*rc(1)-c*rc(2)
                rl(1) = rc(1)
                rl(2) = rc(2)
                rl(3) = rc(3)
            end do

            ra(i) = -rc(3)+ac
            rv(i) = -rc(2)
            rd(i) = -rc(1)

            al = ac

        end do
        return

    end subroutine rnmk

    subroutine ranmk(acc,n,dt0,zeta,P,ra)

        integer, intent(in) :: n
        real(8), intent(in) :: dt0, zeta
        real(8), intent(in) :: acc(:), P
        real(8), intent(out) :: ra(:)

        real(8), allocatable :: rv(:),rd(:)

        allocate(rv(n))
        allocate(rd(n))

        call rnmk(acc,n,dt0,zeta,P,ra,rv,rd)

        deallocate(rv)
        deallocate(rd)

        return
    end subroutine ranmk

    subroutine rfreq(acc,n,dt,zeta,P,ra,rv,rd)

        integer, intent(in) :: n
        real(C_DOUBLE), intent(in) :: dt, zeta, P
        real(C_DOUBLE), intent(in) :: acc(:)
        real(C_DOUBLE), intent(out) :: ra(:),rv(:),rd(:)

        complex(C_DOUBLE_COMPLEX), allocatable :: af(:), raf(:), rvf(:), rdf(:)
        real(C_DOUBLE), allocatable :: a0(:), w(:)
        integer :: i, j, Nfft
        complex(C_DOUBLE_COMPLEX) :: IONE=cmplx(0.d0,1.d0)
        real(C_DOUBLE) :: w0i2, w0iwj, wj2, iNfft, w0
        type(C_PTR) :: plan, iplan

        Nfft = nextpow2(n)*4
        iNfft = 1.d0/dble(Nfft)

        allocate(a0(Nfft))
        allocate(af(Nfft))
        allocate(raf(Nfft))
        allocate(rvf(Nfft))
        allocate(rdf(Nfft))
        allocate(w(Nfft))

        call fftfreqs(Nfft,1.d0/dt,w)
        w = w*6.283185307179586D0
        w0 = 6.283185307179586D0/P

        ra = 0.d0
        rv = 0.d0
        rd = 0.d0
        a0 = 0.d0
        a0(1:n) = acc

        plan  = fftw_plan_dft_r2c_1d(Nfft, a0, af, FFTW_ESTIMATE)
        iplan = fftw_plan_dft_c2r_1d(Nfft, af, ra, FFTW_ESTIMATE)
        call fftw_execute_dft_r2c(plan, a0, af)

        w0i2 = w0*w0
        do j = 1, Nfft, 1
            w0iwj = w0*w(j)
            wj2 = w(j)*w(j)
            raf(j) = af(j)*(w0i2+2.D0*zeta*w0iwj*IONE)/&
                    (w0i2-wj2+2.d0*zeta*w0iwj*IONE)*iNfft
            rvf(j) = af(j)*(-w(j)*IONE/(w0i2-wj2+2.d0*zeta*w0iwj*IONE))*iNfft
            rdf(j) = af(j)*(-1.d0/(w0i2-wj2+2.d0*zeta*w0iwj*IONE))*iNfft
        end do
        call fftw_execute_dft_c2r(iplan, raf, ra)
        call fftw_execute_dft_c2r(iplan, rvf, rv)
        call fftw_execute_dft_c2r(iplan, rdf, rd)

        call fftw_destroy_plan(plan)
        call fftw_destroy_plan(iplan)

        deallocate( a0)
        deallocate( af)
        deallocate(raf)
        deallocate(rvf)
        deallocate(rdf)
        deallocate(  w)

        return

    end subroutine rfreq

    subroutine rafreq(acc,n,dt,zeta,P,ra)

        integer, intent(in) :: n
        real(C_DOUBLE), intent(in) :: dt, zeta, P
        real(C_DOUBLE), intent(in) :: acc(:)
        real(C_DOUBLE), intent(out) :: ra(:)

        complex(C_DOUBLE_COMPLEX), allocatable :: af(:), raf(:)
        real(C_DOUBLE), allocatable :: a0(:), w(:)
        integer :: i, j, Nfft
        complex(C_DOUBLE_COMPLEX) :: IONE=cmplx(0.d0,1.d0)
        real(C_DOUBLE) :: w0i2, w0iwj, wj2, iNfft, w0
        type(C_PTR) :: plan, iplan

        Nfft = nextpow2(n)*4
        iNfft = 1.d0/dble(Nfft)

        allocate(a0(Nfft))
        allocate(af(Nfft))
        allocate(raf(Nfft))
        allocate(w(Nfft))

        call fftfreqs(Nfft,1.d0/dt,w)
        w = w*6.283185307179586D0
        w0 = 6.283185307179586D0/P

        ra = 0.d0
        a0 = 0.d0
        a0(1:n) = acc

        plan  = fftw_plan_dft_r2c_1d(Nfft, a0, af, FFTW_ESTIMATE)
        iplan = fftw_plan_dft_c2r_1d(Nfft, af, ra, FFTW_ESTIMATE)
        call fftw_execute_dft_r2c(plan, a0, af)

        w0i2 = w0*w0
        do j = 1, Nfft, 1
            w0iwj = w0*w(j)
            wj2 = w(j)*w(j)
            raf(j) = af(j)*(w0i2+2.D0*zeta*w0iwj*IONE)/&
                    (w0i2-wj2+2.d0*zeta*w0iwj*IONE)*iNfft
        end do
        call fftw_execute_dft_c2r(iplan, raf, ra)

        call fftw_destroy_plan(plan)
        call fftw_destroy_plan(iplan)

        deallocate( a0)
        deallocate( af)
        deallocate(raf)
        deallocate(  w)

        return

    end subroutine rafreq

    subroutine rmixed(acc,n,dt,zeta,P,ra,rv,rd)

        integer, intent(in) :: n
        real(C_DOUBLE), intent(in) :: dt, zeta, P
        real(C_DOUBLE), intent(in) :: acc(:)
        real(C_DOUBLE), intent(out) :: ra(:),rv(:),rd(:)

        if (P<=MPR*dt) then
            call rfreq(acc,n,dt,zeta,P,ra,rv,rd)
        else
            call rnmk(acc,n,dt,zeta,P,ra,rv,rd)
        end if
        
    end subroutine rmixed

    subroutine ramixed(acc,n,dt,zeta,P,ra)

        integer, intent(in) :: n
        real(C_DOUBLE), intent(in) :: dt, zeta, P
        real(C_DOUBLE), intent(in) :: acc(:)
        real(C_DOUBLE), intent(out) :: ra(:)

        if (P<=MPR*dt) then
            call rafreq(acc,n,dt,zeta,P,ra)
        else
            call ranmk(acc,n,dt,zeta,P,ra)
        end if
        
    end subroutine ramixed

    subroutine fitspectra(acc,n,dt,zeta,P,nP,SPAT,a,tol,mit)

        integer, intent(in) :: n, nP, mit
        real(C_DOUBLE), intent(in) :: dt, zeta, tol
        real(C_DOUBLE), intent(in) :: acc(:), P(:), SPAT(:)
        real(C_DOUBLE), intent(out) :: a(:)

        real(C_DOUBLE), allocatable :: SPA(:), R(:), Rf(:), Pf(:)
        integer(C_INT), allocatable :: INDSPA(:)

        real(C_DOUBLE) :: aerror, merror, peak0, peak
        complex(C_DOUBLE_COMPLEX), allocatable :: af(:), afs(:)
        real(C_DOUBLE), allocatable :: a0(:), f(:)
        integer :: i, j, Nfft, NPf, IPf1, IPf2, iter, pk
        complex(C_DOUBLE_COMPLEX) :: IONE=cmplx(0.d0,1.d0)
        real(C_DOUBLE) :: pfi, iNfft, p0, pe0, dp, phi, t, ra
        type(C_PTR) :: plan, iplan

        peak0 = maxval(abs(acc), dim=1)
        Nfft = nextpow2(n)*2
        iNfft = 1.d0/dble(Nfft)

        allocate(a0(Nfft))
        allocate(af(Nfft))
        allocate(afs(Nfft))
        allocate(f(Nfft))
        allocate(Pf(Nfft/2))
        allocate(Rf(Nfft/2))
        a = acc

        call fftfreqs(Nfft,1.d0/dt,f)
        Pf(2:Nfft/2) = 1.d0/f(2:Nfft/2)
        Pf(1) = 2.d0*Pf(2)

        call decrfindfirst(Pf,Nfft/2,P(nP),IPf1)
        call decrfindlast(Pf,Nfft/2,P(1), IPf2)
        
        NPf = IPf2-IPf1+1

        a0 = 0.d0
        a0(1:n) = acc

        plan  = fftw_plan_dft_r2c_1d(Nfft, a0, af, FFTW_ESTIMATE)
        iplan = fftw_plan_dft_c2r_1d(Nfft, af, a0, FFTW_ESTIMATE)
        call fftw_execute_dft_r2c(plan, a0, af)

        allocate(SPA(nP))
        allocate(R(nP))
        allocate(INDSPA(nP))

        call spamixed(acc,n,dt,zeta,P,nP,SPA,INDSPA)
        call error(abs(SPA),SPAT,nP,aerror,merror)
        R = SPAT/abs(SPA)
        call decrlininterp(P,R,nP,Pf(IPf1:IPf2),Rf(IPf1:IPf2),NPf)
        write(unit=*, fmt="(A29,2F8.4)") "Initial Error: ",aerror,merror

        iter = 1
        do while ( (aerror>tol .or. merror>1.d0*tol) .and. iter<=mit )
            
            j = IPf2
            do i = 2, nP-1, 1
                p0 = p(i)
                t = dt*INDSPA(i)-dt
                dp = min(p0-P(i-1),P(i+1)-p0)

                do while ( Pf(j)<p0+0.5d0*dp .and. j>IPf1)
                    pe0 = Pf(j)
                    phi = atan2(aimag(af(j)),real(af(j)))
                    ra = rsimple(pe0,p0,phi,t,zeta)
                    if ( sign(1.d0,ra)*sign(1.d0,SPA(i)) < 0.d0) then
                        Rf(j) = 1.d0/Rf(j)
                    end if
                    j = j - 1
                end do
            end do

            af(IPf1:IPf2) = af(IPf1:IPf2)*Rf(IPf1:IPf2)
            afs = af
            call fftw_execute_dft_c2r(iplan, afs, a0)
            a = a0(1:n)*iNfft

            call adjustpeak(a,n,peak0)
            call adjustbaseline(a,n,dt)
            call adjustpeak(a,n,peak0)

            call spamixed(a,n,dt,zeta,P,nP,SPA,INDSPA)
            call error(abs(SPA),SPAT,nP,aerror,merror)
            R = SPAT/abs(SPA)
            call decrlininterp(P,R,nP,Pf(IPf1:IPf2),Rf(IPf1:IPf2),NPf)
            write(unit=*, fmt="(A11,I4,A14,2F8.4)") "Error After",iter,"  Iterations: ",aerror,merror
            iter = iter + 1
        end do

        call fftw_destroy_plan(plan)
        call fftw_destroy_plan(iplan)

        deallocate(SPA)
        deallocate(R)
        deallocate(Pf)
        deallocate(Rf)
        deallocate(INDSPA)
        deallocate(a0)
        deallocate(af)
        deallocate(afs)
        deallocate(f)
        
    end subroutine fitspectra

    subroutine adjustpeak(a,n,peak0)
        integer, intent(in) :: n
        real(C_DOUBLE), intent(inout) :: a(:)
        real(C_DOUBLE), intent(in) :: peak0

        integer :: pk = 0, i
        real(C_DOUBLE) :: peak = 0.d0

        do i = 1, n, 1
            if ( abs(a(i))>peak ) then
                peak = a(i)
                pk = i
            end if
            if ( a(i)>peak0 ) then
                a(i) = peak0
            elseif ( a(i)<-peak0 ) then
                a(i) = -peak0
            end if
        end do

        if ( peak<peak0 ) then
            a(pk) = sign(peak0,peak)
        end if
        
    end subroutine adjustpeak

    subroutine adjustbaseline(a,n,dt)
        integer, intent(in) :: n
        real(C_DOUBLE), intent(inout) :: a(:)
        real(C_DOUBLE), intent(in) :: dt

        real(C_DOUBLE), allocatable :: v(:), d(:)
        integer, allocatable :: tp(:)
        integer :: ntp = 4, pl = 2, ph, i

        ph = pl + ntp - 1

        allocate(v(n))
        allocate(d(n))
        allocate(tp(ntp))

        do i = 1, ntp, 1
            tp(i) = int(dble(i)/dble(ntp)*dble(n))
        end do

        call ratacc2vd(a,v,d,n,dt,0.d0,0.d0)
        call targetdc(a,d,n,tp,ntp,ph,pl,dt,0.d0,0.d0)

        deallocate(v)
        deallocate(d)
        deallocate(tp)

        return
    end subroutine adjustbaseline

    subroutine adjustspectra(acc,n,dt,zeta,P,nP,SPAT,a,tol,mit)

        integer, intent(in) :: n, nP, mit
        real(C_DOUBLE), intent(in) :: dt, zeta, tol
        real(C_DOUBLE), intent(in) :: acc(:), P(:), SPAT(:)
        real(C_DOUBLE), intent(out) :: a(:)

        real(C_DOUBLE), allocatable :: SPA(:), dR(:), ra(:,:,:)
        real(C_DOUBLE), allocatable :: M(:,:), W(:,:)
        integer(C_INT), allocatable :: INDSPA(:), INDSPAp(:)

        real(C_DOUBLE) :: aerror, merror, peak0
        integer :: i,j,iter

        allocate(SPA(nP))
        allocate(dR(nP))
        allocate(INDSPA(nP))
        allocate(INDSPAp(nP))
        allocate(ra(n,nP,nP))
        allocate(M(nP,nP))
        allocate(W(n,nP))

        peak0 = maxval(abs(acc), dim=1)
        a = acc
        INDSPAp = -1

        iter = 1
        call spamixed(a,n,dt,zeta,P,nP,SPA,INDSPA)
        call errora(abs(SPA),SPAT,nP,aerror,merror)
        write(unit=*, fmt="(A29,2F8.4)") "Initial Error: ",aerror,merror
        do while ( (aerror>tol .or. merror>1.d0*tol) .and. iter<=mit )

            dR = SPA*(SPAT/abs(SPA)-1.d0)/SPAT

            do i = 1, nP, 1
                if ( INDSPA(i) /= INDSPAp(i) ) then
                    call wfunc( n,dt,INDSPA(i),P(i),zeta,W(:,i) )
                end if
            end do

            do i = 1, nP, 1
                do j = 1, nP, 1
                    if ( INDSPA(j) /= INDSPAp(j) ) then
                        call ramixed(W(:,j),n,dt,zeta,P(i),ra(:,i,j))
                    end if
                    M(i,j) = ra(INDSPA(i),i,j)/SPAT(i)
                    if ( i /= j ) then
                        M(i,j) = M(i,j)*0.618
                    end if
                end do
            end do

            call leastsqs(M,dR,nP,nP)

            do i = 1, nP, 1
                a = a + dR(i)*W(:,i)
            end do

            call adjustpeak(a,n,peak0)

            INDSPAp = INDSPA
            call spamixed(a,n,dt,zeta,P,nP,SPA,INDSPA)
            call errora(abs(SPA),SPAT,nP,aerror,merror)
            write(unit=*, fmt="(A11,I4,A14,2F8.4)") "Error After",iter,"  Iterations: ",aerror,merror
            iter = iter + 1
            
        end do

        deallocate(SPA)
        deallocate(dR)
        deallocate(INDSPA)
        deallocate(INDSPAp)
        deallocate(ra)
        deallocate(M)
        deallocate(W)
        
    end subroutine adjustspectra

    subroutine wfunc(n,dt,itm,P,zeta,wf)

        integer, intent(in) :: n, itm
        real(8), intent(in) :: dt, P, zeta
        real(8), intent(out) :: wf(:)

        real(8) :: tm, w, f, tmp1, gamma, deltaT
        real(8), allocatable :: tmp2(:)
        integer :: i

        allocate(tmp2(n))

        tm = dble(itm-1)*dt
        w = pi2/P
        f = 1.d0/P

        tmp1 = sqrt(1.d0-zeta**2)
        gamma = 1.178d0*(f*tmp1)**(-0.93d0)
        deltaT = atan(tmp1/zeta)/(w*tmp1)

        tmp2 = [ ( ( dble(i-1)*dt-tm+deltaT ),i=1,n ) ]

        wf = cos(w*tmp1*tmp2)*exp(-(tmp2/gamma)**2)

        deallocate(tmp2)

        return
    end subroutine wfunc

    function rsimple(pe0,p0,phi,t,zeta)

        real(C_DOUBLE), intent(in) :: pe0,p0,phi,t,zeta
        real(C_DOUBLE) :: rsimple

        real(C_DOUBLE) :: we, w
        real(C_DOUBLE) :: we2, w2, zeta2
        real(C_DOUBLE) :: we3, w3, zeta3
        real(C_DOUBLE) :: we4, w4, zeta4
        real(C_DOUBLE) :: sinphi, cosphi
        real(C_DOUBLE) :: two = 2.d0, three = 3.d0, four = 4.d0
        real(C_DOUBLE) :: eight = 8.d0, ten = 1.d1, twelve = 12.d0

        we = pi2/pe0
        w = pi2/p0
        we2 = we*we
        w2 = w*w
        we3 = we2*we
        w3 = w2*w
        we4 = we3*we
        w4 = w3*w

        zeta2 = zeta*zeta
        zeta3 = zeta2*zeta
        zeta4 = zeta3*zeta

        sinphi = sin(phi)
        cosphi = cos(phi)

        rsimple = cos(we*t+phi) - exp(-w*zeta*t)*((four*w*we3*zeta &
            -four*w*we3*zeta3)*exp(w*zeta*t)*sin(we*t+phi)+((two*we4 &
            -two*w2*we2)*zeta2-two*we4+two*w2*we2)*exp(w*zeta*t)*cos(we*t &
            +phi)+sqrt(four*w2-four*w2*zeta2)*(four*cosphi*w*we2*zeta3 &
            +two*sinphi*we3*zeta2+(cosphi*w3-three*cosphi*w*we2) &
            *zeta-sinphi*we3+sinphi*w2*we)*sin(sqrt(four*w2-four*w2 &
            *zeta2)*t/two)+(eight*cosphi*w2*we2*zeta4+four*sinphi*w &
            *we3*zeta3+(two*cosphi*w4-ten*cosphi*w2*we2)*zeta2-four &
            *sinphi*w*we3*zeta+two*cosphi*w2*we2-two*cosphi*w4) &
            *cos(sqrt(four*w2-four*w2*zeta2)*t/two))/(eight*w2*we2*zeta4 &
            +(two*we4-twelve*w2*we2+two*w4)*zeta2-two*we4+four*w2*we2 &
            -two*w4)

    end function rsimple

    subroutine targetdc(a,td,n,tp,ntp,ph,pl,dt,v0,d0)

        integer(4), intent(in) :: n
        real(8), intent(in) :: dt,v0,d0
        real(8), intent(in) :: td(n)
        real(8), intent(out) :: a(n)
        integer(4), intent(in) :: ntp,ph,pl
        integer(4), intent(in) :: tp(ntp)

        integer :: i,j,k,pn
        real(8), allocatable :: M(:,:), b(:), tc(:)
        real(8), allocatable :: v(:), d(:), t(:)

        pn = ph - pl + 1
        if ( pn>ntp ) pn = ntp

        allocate(M(ntp,pn))
        allocate(b(ntp))
        allocate(tc(ntp))
        allocate(t(n))
        allocate(v(n))
        allocate(d(n))

        t = [ (((i-1)*dt),i=1,n) ]
        call acc2vd(a,v,d,n,dt,v0,d0)

        do k = 1, ntp, 1
            j = tp(k)
            tc(k) = t(j)
            b(k)  = td(j)-d(j)
            do i = pl, pl+pn-1, 1
                M(k,i-pl+1) = dpower(tc(k),i,0)
            end do
        end do

        call leastsqs(M,b,ntp,pn)

        do k = 1, n, 1
            do i = pl, pl+pn-1, 1
                a(k) = a(k)+b(i-pl+1)*dpower(t(k),i,2)
            end do
        end do

        deallocate(M)
        deallocate(b)
        deallocate(tc)
        deallocate(t)
        deallocate(v)
        deallocate(d)

        return
    end subroutine targetdc

end module spectra
